package com.google.android.apps.skipbot;

public class SkipperRobotConfig implements SkipperRobotConfigInterface {
  private static final int PIN_INDEX = 0;
  private static final int DOWN_INDEX = 1;
  private static final int UP_INDEX = 2;
  private static final int CALIBRATION_SHIFT_INDEX = 3;
  private static final double SCALE_FACTOR = 1.0;

  private final int NUM_SERVOS = 18;
  private final int NUM_LEGS = 6;

  private final int configArray[][] = {
      // Limb 5
      {2, 0, 180, 7}, // hip 0
      {3, 0, 180, 10}, // shoulder 1
      {4, 180, 0, 15}, // elbow 2
      // Limb 4
      {5, 0, 180, 1}, // hip 3
      {6, 0, 180, -3}, // shoulder 4
      {7, 180, 0, 10}, // elbow 5
      // Limb 3
      {8, 0, 180, 2}, // hip 6
      {9, 0, 180, 0}, // shoulder 7
      {10, 180, 0, 8}, // elbow 8
      // Limb 2
      {44, 0, 180, -4}, // hip 9
      {46, 180, 0, 3}, // shoulder 10
      {48, 0, 180, -7}, // elbow 11
      // Limb 1
      {38, 0, 180, 3}, // hip 12
      {40, 180, 0, 7}, // shoulder 13
      {42, 0, 180, -10}, // elbow 14
      // Limb 0
      {32, 0, 180, -6}, // hip 15
      {34, 180, 0, 12}, // shoulder 16
      {36, 0, 180, 0} // elbow 17
      };

  // The position of the hips for each leg relative to the center bottom of the
  // robot.
  private final double hipPosition[][] = { {1.557, 2.696, 1.0}, {3.114, 0.0, 1.0},
      {1.557, -2.696, 1.0}, {-1.557, -2.696, 1.0}, {-3.114, 0.0, 1.0}, {-1.557, 2.696, 1.0}};

  // Relative position of the shoulder from the hip.
  private final double hipToShoulderOffset[][] = { {-0.6, 0.77, 0.0}, {-0.6, 0.77, 0.0},
      {-0.6, 0.77, 0.0}, {0.6, 0.77, 0.0}, {0.6, 0.77, 0.0}, {0.6, 0.77, 0.0}};

  private final boolean flipped[] = {
      // Limb 5
      false, // hip 0
      false, // shoulder 1
      false, // elbow 2
      // Limb 4
      false, // hip 3
      false, // shoulder 4
      false, // elbow 5
      // Limb 3
      false, // hip 6
      false, // shoulder 7
      false, // elbow 8
      // Limb 2
      false, // hip 9
      true, // shoulder 10
      true, // elbow 11
      // Limb 1
      false, // hip 12
      true, // shoulder 13
      true, // elbow 14
      // Limb 0
      false, // hip 15
      true, // shoulder 16
      true // elbow 17
      };

  // Length between the Shoulder servo and the Elbow servo in inches.
  private final double UPPER_ARM_LENGTH = 2.828;
  // Length between the Elbow servo and the end point of the leg in inches.
  private final double LOWER_ARM_LENGTH = 4.7;
  // Angle between the line from the elbo servo axis to the tip and "vertical"
  // atan(.4/4.7);
  private final double LEG_ANGLE_OFFSET = 0.085; // radians;

  private final int[] calibrationShiftOverride;

  public SkipperRobotConfig() {
    this.calibrationShiftOverride = null;
  }

  public SkipperRobotConfig(int[] calibrationShiftOverride) {
    this.calibrationShiftOverride = calibrationShiftOverride;
  }

  @Override
  public int getPin(int servoIndex) {
    return configArray[servoIndex][PIN_INDEX];
  }

  @Override
  public int getDownAngle(int servoIndex) {
    return configArray[servoIndex][DOWN_INDEX];
  }

  @Override
  public int getUpAngle(int servoIndex) {
    return configArray[servoIndex][UP_INDEX];
  }

  @Override
  public int getNumServos() {
    return NUM_SERVOS;
  }

  @Override
  public int getCalibrationShift(int servoIndex) {
    if (calibrationShiftOverride != null) {
      return calibrationShiftOverride[servoIndex];
    } else {
      return configArray[servoIndex][CALIBRATION_SHIFT_INDEX];
    }
  }

  public boolean getDirectionFlipped(int servoIndex) {
    return flipped[servoIndex];
  }

  @Override
  public double[] getHipPosition(int legIndex) {
    return hipPosition[legIndex];
  }

  @Override
  public double[] getHipToShoulderOffset(int legIndex) {
    return hipToShoulderOffset[legIndex];
  }

  @Override
  public double getUpperArmLength(int legIndex) {
    return UPPER_ARM_LENGTH;
  }

  @Override
  public double getLowerArmLength(int legIndex) {
    return LOWER_ARM_LENGTH;
  }

  @Override
  public double getLegAngleOffset(int legIndex) {
    return LEG_ANGLE_OFFSET;
  }

  @Override
  public int getHipServoIndex(int legIndex) {
    return (NUM_LEGS - legIndex - 1) * 3;
  }

  @Override
  public int getShoulderServoIndex(int legIndex) {
    return (NUM_LEGS - legIndex - 1) * 3 + 1;
  }

  @Override
  public int getElbowServoIndex(int legIndex) {
    return (NUM_LEGS - legIndex - 1) * 3 + 2;
  }

  @Override
  public int getNumLegs() {
    return NUM_LEGS;
  }

  // Looking from the top of the robot is the leg on the right side of the hip
  // axis?
  @Override
  public boolean isLegOnRightOfHip(int legIndex) {
    return legIndex > 2;
  }

  @Override
  public double getScaleFactor() {
    return SCALE_FACTOR;
  }
}
